from OpenGL.GLUT import *
from OpenGL.GLU import *
from OpenGL.GL import *
from vertices import *
import numpy as np
import math
import KinectTracker2Hands
from collections import deque
from SvmWrap import *

comList = [ \
"v.addPolygon([(0.57735000000000003, -0.57735000000000003, -0.57735000000000003),(0.934172, -0.35682199999999997, 0.0),(0.934172, 0.35682199999999997, 0.0),(0.57735000000000003, 0.57735000000000003, -0.57735000000000003),(0.35682199999999997, 0.0, -0.934172)])", \
"v.addPolygon([(0.0, 0.934172, -0.35682199999999997),(0.57735000000000003, 0.57735000000000003, -0.57735000000000003),(0.934172, 0.35682199999999997, 0.0),(0.57735000000000003, 0.57735000000000003, 0.57735000000000003),(0.0, 0.934172, 0.35682199999999997)])", \
"v.addPolygon([(0.0, -0.934172, 0.35682199999999997),(0.57735000000000003, -0.57735000000000003, 0.57735000000000003),(0.934172, -0.35682199999999997, 0.0),(0.57735000000000003, -0.57735000000000003, -0.57735000000000003),(0.0, -0.934172, -0.35682199999999997)])", \
"v.addPolygon([(-0.35682199999999997, 0.0, 0.934172),(-0.57735000000000003, 0.57735000000000003, 0.57735000000000003),(0.0, 0.934172, 0.35682199999999997),(0.57735000000000003, 0.57735000000000003, 0.57735000000000003),(0.35682199999999997, 0.0, 0.934172)])", \
"v.addPolygon([(0.35682199999999997, 0.0, 0.934172),(0.57735000000000003, -0.57735000000000003, 0.57735000000000003),(0.0, -0.934172, 0.35682199999999997),(-0.57735000000000003, -0.57735000000000003, 0.57735000000000003),(-0.35682199999999997, 0.0, 0.934172)])", \
"v.addPolygon([(-0.35682199999999997, 0.0, -0.934172),(-0.57735000000000003, 0.57735000000000003, -0.57735000000000003),(-0.934172, 0.35682199999999997, 0.0),(-0.934172, -0.35682199999999997, 0.0),(-0.57735000000000003, -0.57735000000000003, -0.57735000000000003)])", \
"v.addJoin((-0.35682199999999997, 0.0, -0.934172),(0.35682199999999997, 0.0, -0.934172))", \
"v.addJoin((-0.57735000000000003, 0.57735000000000003, -0.57735000000000003),(0.0, 0.934172, -0.35682199999999997))", \
"v.addJoin((-0.934172, 0.35682199999999997, 0.0),(-0.57735000000000003, 0.57735000000000003, 0.57735000000000003))", \
"v.addJoin((-0.934172, -0.35682199999999997, 0.0),(-0.57735000000000003, -0.57735000000000003, 0.57735000000000003))", \
"v.addJoin((-0.57735000000000003, -0.57735000000000003, -0.57735000000000003),(0.0, -0.934172, -0.35682199999999997))"]
comList.reverse()

name = 'Kinect Test'
v = Vertices()
ws = (320,320)

fl = False
rot = False
rotCent = ()
coord = ()
sel = 0
pl = []
move = False
mx = 0
my = 0
mz = 0
rx, ry, rz = 0, 0, 0
dispList = None
gestQueueR = deque([])
gestQueueL = deque([])

tracker = None

### TODOs
# Perform translations independently
# Use Display lists

def main():
    global tracker, dispList
    #v.fileImport('models/dodecahedron.obj')
    tracker = KinectTracker2Hands.KinectTracker2Hands(False)
    tracker.step()

    glutInit(sys.argv)
    #glutInitWindowSize(ws[0],ws[1])
    glutCreateWindow(name)
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH)
    glClearColor(0.,0.,0.,1.)
    glShadeModel(GL_SMOOTH)
    #glEnable(GL_CULL_FACE)
    glEnable(GL_DEPTH_TEST)
    glEnable(GL_LIGHTING)
    dispList = glGenLists(1)
    lightZeroPosition = [0,0,10,1.]
    lightZeroColor = [1.0,1.0,1.0,1.0]
    glLightfv(GL_LIGHT0, GL_POSITION, lightZeroPosition)
    glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor)
    glLightfv(GL_LIGHT0, GL_AMBIENT, [1,1,1,1])
    glEnable(GL_LIGHT0)
    glutDisplayFunc(display)
    glutKeyboardFunc(keyboard)
    #glutPassiveMotionFunc(mouse)
    glutIdleFunc(loop)
    glMatrixMode(GL_PROJECTION)
    gluPerspective(40.,1.,1.,40.)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    glPushMatrix()
    doComList()
    gluLookAt(0,0,10,
              0,0,0,
              0,1,0)
    glutMainLoop()
    return

def loop():
    global rx,ry,rz
    # Update tracker object
    result,otherHand = tracker.step()
    # See if two hands are being tracked
    #twoHandMode = tracker.isTrackingHandR
    twoHandMode = True
    # Update mouse coordinates
    kinectCursor(result[0][0],result[0][1],result[0][2])

    if twoHandMode:
        # Set coordinates for second hand
        rx,ry,rz = kinect2ScreenSpace(otherHand[0][0],otherHand[0][1],otherHand[0][2])
        kinectGest(result[1],otherHand[1])
    else:
        kinectGest(result[1])
    glutPostRedisplay()

def kinect2ScreenSpace(kx,ky,kz):
    xs = 320
    ys = 240
    zs = 15
    nx = -1*(kx-xs)/xs
    ny = -1*(ky-ys)/ys
    nz = (kz-zs-70)/zs
    sx = nx*5
    sy = ny*5
    sz = nz*2
    return (sx,sy,sz)

def kinectCursor(x,y,z):
    global mx,my,mz
    xs = 320
    ys = 240
    zs = 15
    
    nx = -1*(x-xs)/xs
    ny = -1*(y-ys)/ys
    nz = (z-zs-70)/zs        
    if rot:
        nx *= 5
        ny *= 5
        nz *= 2
        nx -= rotCent[0]
        ny -= rotCent[1]
        nz -= rotCent[2]
        if abs(nx) > 0.7 or abs(ny) > 0.7 or abs(nz) > 0.7:
            rotate(10*nx,[1,0,0])
            rotate(10*ny,[0,1,0])
            rotate(10*nz,[0,0,1])
    else:
        mx = nx*5
        my = ny*5
        mz = nz*2

def kinectGest(curs, mod='Noise'):
    global v,rot,coord,sel,move,pl,gestQueueL,gestQueueR,rotCent
    
    lengthThres = 0.75
    queueLength = 12
    if (len(gestQueueL) >= queueLength) and len(gestQueueR) >= queueLength:
        gestUnique = list(set(gestQueueL))
        gestList = list(gestQueueL)
        maxCountL = 0
        maxGestL = 'None'
        for i in gestUnique:
            c = gestList.count(i)
            if c > maxCountL:
                maxCountL = c
                maxGestL = i
        
        gestUnique = list(set(gestQueueR))
        gestList = list(gestQueueR)
        maxCountR = 0
        maxGestR = 'None'
        for i in gestUnique:
            c = gestList.count(i)
            if c > maxCountR:
                maxCountR = c
                maxGestR = i

        # Add vertex (C: one_finger, M: one_finger)
        # Close vertex (polygon) (C: spread_fingers, M: one_finger
        # Translate (vertex) (C:one_finger, M:spread_fingers )
        # Rotate (C:spread_fingers, M: spread_fingers)
        # Remove vertex (C: one_finger, M: fist)
        # Select vertex (C: spread_fingers, M: fist)
        # Cursor (C: fist, M:any)
        # (Add dodecahedron)
        if maxCountL >= queueLength*lengthThres and maxCountR >= queueLength*lengthThres:
            # Add vertex
            if maxGestL == 'one_finger' and maxGestR == 'one_finger' and curs == 'fist':
                if sel == 3:
                    loc = coord
                    sel = 0
                else:
                    loc = getMouse()                
                pl.append(loc)
                v.addVertex(loc)
                if len(pl) > 1:
                    v.addEdge(pl[-2],pl[-1])
                gestQueueL = deque([])
            # Close vertex
            elif maxGestL == 'spread_fingers' and maxGestR == 'one_finger' and curs == 'fist':
                if sel == 3:
                    loc = coord
                    sel = 0
                else:
                    loc = getMouse()
                pl.append(loc)
                v.addVertex(loc)
                if len(pl) == 1:
                    v.addVertex(pl[0])
                elif len(pl) == 2:
                    v.addJoin(pl[0],pl[1])
                elif len(pl) > 2:
                    v.addPolygon(pl)
                pl = []
                gestQueueL = deque([])
            # Select
            elif maxGestL == 'spread_fingers' and maxGestR == 'fist':
                if sel == 0 or sel == 3:
                    sel = 1
                elif sel == 1 and curs == 'fist':
                    gestQueueL = deque([])
                    sel = 2
            # Remove
            elif maxGestL == 'one_finger' and maxGestR == 'fist' and curs == 'fist':
                if sel == 3:
                    v.removeVertex(coord)
                    sel = 0
                    gestQueueL = deque([])
            # Translate
            elif maxGestL == 'one_finger' and maxGestR == 'spread_fingers':
                if not move:
                    move = True
                elif curs == 'fist':
                    move = False
                    sel = 0
                    gestQueueL = deque([])
            # Rotate
            elif maxGestL == 'spread_fingers' and maxGestR == 'spread_fingers' and not rot:
                rotCent = (mx,my,mz)
                rot = True
        # Turn off rotation, independent of maxGests
        if rot and curs == 'fist':
            rot = False
    # Maintain queues
    if len(gestQueueL) > 0 and len(gestQueueL) >= queueLength:
        gestQueueL.popleft()
    if len(gestQueueR) > 0 and len(gestQueueR) >= queueLength:
        gestQueueR.popleft()
    gestQueueL.append(curs)
    gestQueueR.append(mod)

def display():
    global v,sel,coord
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
    if move and sel == 3:
        v.moveVertex(coord,getMouse())
        coord = getMouse()
    ml = drawMouse()
    drawObject()
    if sel != 0:
        vertList = v.vertexList()
        neigh = ()
        if sel == 1:
            coord = getMouse()
            neigh = getNeigh(coord,vertList)
        elif sel == 2:
            neigh = getNeigh(coord,vertList)
            coord = neigh
            sel = 3
        elif sel == 3:
            neigh = coord
        drawVertex(neigh,True)
    v.modif = False
    glutSwapBuffers()
    return


def doComList():
    while len(comList) > 0:
        eval(comList.pop())
    display()

def drawObject():
    global v,dispList,start
    if v.display:
        glNewList(dispList,GL_COMPILE)
        polyList = v.polygonList()
        for poly in polyList:
            drawPolygon(poly[0],poly[1])
        edgeList = v.edgeList()
        for edge in edgeList:
            drawEdge(edge[0],edge[1])
        vertList = v.vertexList()
        for vert in vertList:
            drawVertex(vert)
        glEndList()
        v.display = False
    glCallList(dispList)

def drawPolygon(poly,norm):
    glMaterialfv(GL_FRONT,GL_DIFFUSE,[1,1,1,1])
    glBegin(GL_TRIANGLES)
    glNormal(norm[0],norm[1],norm[2])
    for vert in poly:
        glVertex3f(vert[0],vert[1],vert[2])
    glEnd()

def drawEdge(v1,v2):
    vt = np.array(v1) - np.array(v2)
    nc = np.matrix([vt[0],vt[1],vt[2],1])
    yr = math.degrees(math.atan2(nc[0,0],nc[0,2]))
    rm = np.matrix([[math.cos(math.radians(yr)),0,math.sin(math.radians(yr))],[0,1,0], \
    [-math.sin(math.radians(yr)),0,math.cos(math.radians(yr))]])
    co = np.matrix([nc[0,0],nc[0,1],nc[0,2]])
    nc = co*rm
    xr = -math.degrees(math.atan2(nc[0,1],nc[0,2]))
    d = (np.sum((np.array(v1) - np.array(v2))**2))**0.5
    glPushMatrix()
    glTranslatef(v2[0],v2[1],v2[2])
    glRotatef(yr,0,1,0)
    glRotatef(xr,1,0,0)
    glMaterialfv(GL_FRONT,GL_DIFFUSE,[1,0,0,1])
    gluCylinder(gluNewQuadric(),0.03,0.03,d,8,8)
    glPopMatrix()

def drawVertex(v, high=False):
    glPushMatrix()
    glTranslatef(v[0],v[1],v[2])
    if high:
        glMaterialfv(GL_FRONT,GL_DIFFUSE,[0,1,0,1])
        glutSolidSphere(0.08,5,5)
    else:
        glMaterialfv(GL_FRONT,GL_DIFFUSE,[0,0,1,1])
        glutSolidSphere(0.05,5,5)
    glPopMatrix()

def drawMouse():
    glPushMatrix()
    glLoadIdentity()
    gluLookAt(0,0,10,
              0,0,0,
              0,1,0)
    glTranslatef(mx,my,mz)
    glMaterialfv(GL_FRONT,GL_DIFFUSE,[0,1,0,1])
    glutSolidSphere(0.1,5,5)
    
    glPopMatrix()

def getMouse():
    mvmInv = glGetFloatv(GL_MODELVIEW_MATRIX)
    glPopMatrix()
    mvm = np.matrix(glGetFloatv(GL_MODELVIEW_MATRIX))
    mvm = mvm.I
    co = np.matrix([mx,my,mz,1])
    nc = co*mvm
    glPushMatrix()
    glLoadMatrixf(mvmInv)
    return (nc[0,0],nc[0,1],nc[0,2])

def getNeigh(mc, vertList):
    md = -1
    mv = (0,0,0)
    for vert in vertList:
        d = (np.sum((np.array(mc) - np.array(vert))**2))**0.5
        if d < md or md == -1:
            md = d
            mv = vert
    
    return mv

def keyboard(char,x,y):
    global v,comList,fl,rot,coord,sel,move,pl
    if char == 'a':
        rotate(-10,[0,1,0])
    elif char == 'd':
        rotate(10,[0,1,0])
    elif char == 'w':
        rotate(10,[1,0,0])
    elif char == 's':
        rotate(-10,[1,0,0])
    elif char == 'q':
        rotate(10,[0,0,1])
    elif char == 'e':
        rotate(-10,[0,0,1])
    elif char == ' ' and len(comList) > 0:
        eval(comList.pop())
    elif char == 'f':
        fl = not fl
    elif char == 'v':
        if sel == 3:
            loc = coord
        else:
            loc = getMouse()

        pl.append(loc)
        v.addVertex(loc)
        if len(pl) > 1:
            v.addEdge(pl[-2],pl[-1])
    elif char == 'c':
        if sel == 3:
            loc = coord
        else:
            loc = getMouse()
        pl.append(loc)
        v.addVertex(loc)
        if len(pl) == 1:
            v.addVertex(pl[0])
        elif len(pl) == 2:
            v.addJoin(pl[0],pl[1])
        elif len(pl) > 2:
            v.addPolygon(pl)
        pl = []
    elif char == 'x':
        if sel == 0:
            sel = 1
        elif sel == 1:
            sel = 2
        elif sel == 2:
            print 'Fail'
        elif sel == 3:
            sel = 0
    elif char == 'r':
        if sel == 3:
            v.removeVertex(coord)
    elif char == 't':
        move = not move
    elif char == 'm':
        scale(1.2)
    elif char == 'n':
        scale(0.8)
    elif char == 'j':
        translate(-0.5,0,0)
    elif char == 'l':
        translate(0.5,0,0)
    elif char == 'i':
        translate(0,0.5,0)
    elif char == 'k':
        translate(0,-0.5,0)
    elif char == 'u':
        translate(0,0,-0.5)
    elif char == 'o':
        translate(0,0,0.5)
    elif char == 'z':
        exit(0)
    glutPostRedisplay()

def mouse(x,y):
    global mx,my,mz
    
    sx = ((x-(float(ws[0])/2))/ws[0])
    sy = ((y-(float(ws[0])/2))/ws[0])
    
    if rot:
        if fl:
            rotate(sx*10, [1,0,0])
            rotate(sy*10, [0,0,1])
        else:
            rotate(sx*10, [1,0,0])
            rotate(sy*10, [0,1,0])
    else:
        if fl:
            mx = (sx*8)
            mz = (sy*8)
        else:
            mx = (sx*8)
            my = -1*(sy*8)
    glutPostRedisplay()

def rotate(deg, axis):
    glRotatef(deg, axis[0], axis[1], axis[2])
    mvm = glGetFloatv(GL_MODELVIEW_MATRIX)
    glPopMatrix()
    glRotatef(deg, axis[0], axis[1], axis[2])
    glPushMatrix()
    glLoadMatrixf(mvm)


def scale(sc):
    glScalef(sc,sc,sc)
    mvm = glGetFloatv(GL_MODELVIEW_MATRIX)
    glPopMatrix()
    glScalef(sc,sc,sc)
    glPushMatrix()
    glLoadMatrixf(mvm)

def translate(x,y,z):
    glTranslatef(x,y,z)
    mvm = glGetFloatv(GL_MODELVIEW_MATRIX)
    glPopMatrix()
    glTranslatef(x,y,z)
    glPushMatrix()
    glLoadMatrixf(mvm)

if __name__ == '__main__': main()

